
/*需要配置的寄存器*/
// 1.GYRO_MODE ACCEL_MODEL(选择低噪声模式还是低功耗模式)
// 2.GYRO_SRC_CTRL ACCEL_SRC_CTRL(是否开启AAF抗混叠滤波器、是否开启插值采样)
// 3.ACCEL_CONFIG0 GYRO_CONFIG0(选择加速度计和陀螺仪的量程、数据速率)
// 4.INTF_CONFIG_OVRD 选择接口工作模式SPI/I2C等
// 5. SOFT_RESET 复位寄存器，复位所有寄存器

#include "ICM45686.h"

/*片选*/
void ICM45686_CS_Select(void)
{
    HAL_GPIO_WritePin(CS_GPIO_Port, CS_Pin, GPIO_PIN_RESET); // 选择SPI片选引脚
}
void ICM45686_CS_Deselect(void)
{
    HAL_GPIO_WritePin(CS_GPIO_Port, CS_Pin, GPIO_PIN_SET); // 取消选择SPI片选引脚
}

/*传感器初始化函数*/
void ICM45686_Init(void)
{
    /*复位*/
    ICM45686_CS_Select();                                  // 选择SPI片选引脚
    uint8_t reset_cmd[2] = {SOFT_RESET_ADDR, ICM_RESET};   // 复位命令                // 复位寄存器地址
    HAL_SPI_Transmit(&hspi1, reset_cmd, 2, HAL_MAX_DELAY); // 发送复位命令
    HAL_Delay(100);                                        // 等待复位完成
    ICM45686_CS_Deselect();                                // 取消选择SPI片选引脚
    /*配置寄存器*/
    uint8_t config_cmd[2];                                  // 存储配置命令的数组
    config_cmd[0] = PWR_MGMT_1;                             // 电源管理寄存器地址
    config_cmd[1] = (GYRO_MODE_LN | ACCEL_MODE_LN);         // 设置低噪声模式
    ICM45686_CS_Select();                                   // 选择SPI片选引脚
    HAL_SPI_Transmit(&hspi1, config_cmd, 2, HAL_MAX_DELAY); // 发送配置命令
    ICM45686_CS_Deselect();
    HAL_Delay(10);                                          // 等待配置完成
    config_cmd[0] = INTF_CONFIG_OVRD;                       // 接口配置寄存器地址
    config_cmd[1] = SPI_MODE;                               // 设置SPI模式
    ICM45686_CS_Select();                                   // 选择SPI片选引脚
    HAL_SPI_Transmit(&hspi1, config_cmd, 2, HAL_MAX_DELAY); // 发送配置命令
    ICM45686_CS_Deselect();                                 // 取消选择SPI片选引脚
    HAL_Delay(10);                                          // 等待配置完成
    config_cmd[0] = ACCEL_CONFIG0;                          // 加速度计配置寄存器地址
    config_cmd[1] = ACCEL_4G_100hz;                         // 设置加速度计量程±4g 数据刷新速率100Hz
    ICM45686_CS_Select();                                   // 选择SPI片选引脚
    HAL_SPI_Transmit(&hspi1, config_cmd, 2, HAL_MAX_DELAY); // 发送配置命令
    ICM45686_CS_Deselect();                                 // 取消选择SPI片选引脚
    HAL_Delay(10);                                          // 等待配置完成
    config_cmd[0] = GYRO_CONFIG0;                           // 陀螺仪配置寄存器地址
    config_cmd[1] = GYRO_500dps_100hz;                      // 设置陀螺仪量程±500dps 数据刷新速率100Hz
    ICM45686_CS_Select();                                   // 选择SPI片选引脚
    HAL_SPI_Transmit(&hspi1, config_cmd, 2, HAL_MAX_DELAY); // 发送配置命令
    ICM45686_CS_Deselect();                                 // 取消选择SPI片选引脚
    HAL_Delay(10);                                          // 等待配置完成
    config_cmd[0] = GYRO_SRC_CTRL;                          // 陀螺仪抗混叠滤波器与插值采样控制寄存器地址
    config_cmd[1] = GYRO_FIR_InTERP;                        // 设置抗混叠滤波器与插值采样控制寄存器值，开启抗混叠滤波器和插值采样
    ICM45686_CS_Select();                                   // 选择SPI片选引脚
    HAL_SPI_Transmit(&hspi1, config_cmd, 2, HAL_MAX_DELAY); // 发送配置命令
    ICM45686_CS_Deselect();                                 // 取消选择SPI片选引脚
    HAL_Delay(10);                                          // 等待配置完成
    config_cmd[0] = ACCEL_SRC_CTRL;                         // 加速度计抗混叠滤波器与插值采样控制寄存器地址
    config_cmd[1] = ACCEL_FIR_InTERP;                       // 设置抗混叠滤波器与插值采样控制寄存器值，开启抗混叠滤波器和插值采样
    ICM45686_CS_Select();                                   // 选择SPI片选引脚
    HAL_SPI_Transmit(&hspi1, config_cmd, 2, HAL_MAX_DELAY); // 发送配置命令
    ICM45686_CS_Deselect();                                 // 取消选择SPI片选引脚
    HAL_Delay(10);                                          // 等待配置完成
    config_cmd[0] = INT1_CONFIG;                            // 中断配置寄存器地址
    config_cmd[1] = INT_CONFIG;                             // 设置中断配置寄存器值，配置中断引脚的工作模式
    ICM45686_CS_Select();                                   // 选择SPI片选引脚
    HAL_SPI_Transmit(&hspi1, config_cmd, 2, HAL_MAX_DELAY); // 发送配置命令
    ICM45686_CS_Deselect();                                 // 取消选择SPI片选引脚
    HAL_Delay(10);                                          // 等待配置完成
    /*查找设备ID*/
    uint8_t whoami_data;                       // 存储设备ID的变量
    ICM45686_ReadData(WHO_AM_I, &whoami_data); // 读取设备ID数据
    if (whoami_data == DEVICE_ID)              // 检查设备ID是否正确
    {
        HAL_UART_Transmit(&huart2, (uint8_t *)"ICM-45686 Found!\r\n", 18, HAL_MAX_DELAY); // 打印设备找到信息
    }
    else
    {
        HAL_UART_Transmit(&huart2, (uint8_t *)"ICM-45686 Not Found\r\n", 20, HAL_MAX_DELAY); // 打印设备未找到信息
        HAL_UART_Transmit(&huart2, &whoami_data, 1, HAL_MAX_DELAY);                          // 打印读取的设备ID
        return;
    }
    /*检测关键参数是否写入成功*/
    uint8_t read_data;                           // 存储读取命令
    ICM45686_ReadData(GYRO_CONFIG0, &read_data); // 读取陀螺仪配置寄存器数据
    if (read_data == GYRO_500dps_100hz)          // 检查陀螺仪配置是否正确
    {
        HAL_UART_Transmit(&huart2, (uint8_t *)"GYRO_CONFIG0 OK\r\n", 17, HAL_MAX_DELAY); // 打印陀螺仪配置成功信息
    }
    else
    {
        HAL_UART_Transmit(&huart2, (uint8_t *)"GYRO_CONFIG0 ERROR\r\n", 21, HAL_MAX_DELAY); // 打印陀螺仪配置失败信息
        return;
    }
    /*配置完成，打印设备ID*/
    HAL_UART_Transmit(&huart2, (uint8_t *)"ICM-45686 Initialized\r\n", 22, HAL_MAX_DELAY); // 打印初始化完成信息
}
/*读取加速度计和陀螺仪数据函数*/
void ICM45686_decodedata(int16_t *accel_data, int16_t *gyro_data)
{
    uint8_t data[12]; // 存储读取数据的数组
    ICM45686_ReadData(ACCEL_DATA_XH, data);
    ICM45686_ReadData(ACCEL_DATA_XL, data + 1);
    ICM45686_ReadData(ACCEL_DATA_YH, data + 2);
    ICM45686_ReadData(ACCEL_DATA_YL, data + 3);
    ICM45686_ReadData(ACCEL_DATA_ZH, data + 4);
    ICM45686_ReadData(ACCEL_DATA_ZL, data + 5);
    ICM45686_ReadData(GYRO_DATA_XH, data + 6);
    ICM45686_ReadData(GYRO_DATA_XL, data + 7);
    ICM45686_ReadData(GYRO_DATA_YH, data + 8);
    ICM45686_ReadData(GYRO_DATA_YL, data + 9);
    ICM45686_ReadData(GYRO_DATA_ZH, data + 10);
    ICM45686_ReadData(GYRO_DATA_ZL, data + 11); // 读取加速度计和陀螺仪数据
    /*解析数据*/
    //大端序↓
    // accel_data[0] = (int16_t)((data[0] << 8) | data[1]);  // 加速度计X轴数据
    // accel_data[1] = (int16_t)((data[2] << 8) | data[3]);  // 加速度计Y轴数据
    // accel_data[2] = (int16_t)((data[4] << 8) | data[5]);  // 加速度计Z轴数据
    // gyro_data[0] = (int16_t)((data[6] << 8) | data[7]);   // 陀螺仪X轴数据
    // gyro_data[1] = (int16_t)((data[8] << 8) | data[9]);   // 陀螺仪Y轴数据
    // gyro_data[2] = (int16_t)((data[10] << 8) | data[11]); // 陀螺仪Z轴数据
    //小端序↓
    accel_data[0] = (int16_t)((data[1] << 8) | data[0]);  // 加速度计X轴数据
    accel_data[1] = (int16_t)((data[3] << 8) | data[2]);  // 加速度计Y轴数据
    accel_data[2] = (int16_t)((data[5] << 8) | data[4]);  // 加速度计Z轴数据
    gyro_data[0] = (int16_t)((data[7] << 8) | data[6]);   // 陀螺仪X轴数据
    gyro_data[1] = (int16_t)((data[9] << 8) | data[8]);   // 陀螺仪Y轴数据
    gyro_data[2] = (int16_t)((data[11] << 8) | data[10]); // 陀螺仪Z轴数据
}
void ICM45686_ReadData(uint8_t addr, uint8_t *data)
{
    uint8_t read_cmd[2] = {(addr | (1 << 7)), 0};                          // 读取寄存器地址
    uint8_t tempdata[2] = {0};                                             // 存储读取数据的数组
    ICM45686_CS_Select();                                                  // 选择SPI片选引脚
    HAL_SPI_TransmitReceive(&hspi1, read_cmd, tempdata, 2, HAL_MAX_DELAY); // 发送读取命令并接收数据
    ICM45686_CS_Deselect();                                                // 取消选择SPI片选引脚
    *data = tempdata[1];                                                    // 获取读取的数据
}
